#include "./status_task.h"
#include "../driver/cmd_uart.h"
#include "../driver/ble_uart.h"
#include "../driver/log_uart.h"
#include "../driver/sensor.h"
#include "../driver/ds18b20.h"
#include "../driver/motor_drv.h"
#include "../driver/tube.h"
#include "../driver/fridge.h"
#include "../driver/robot_arm1.h"
#include "../driver/robot_arm2.h"

namespace task {

using namespace utils;
using namespace driver;

template <typename... Ts>
static void write(uint16_t req, Ts&&... ts) {
    cmdUart->write(req, std::forward<Ts>(ts)...);
    bleUart->write(req, std::forward<Ts>(ts)...);
    logUart->write(req, std::forward<Ts>(ts)...);
}

constexpr uint16_t Version = 0x06;

void StatusTask::run() {
    cmdUart->throw_log(0, "软件版本:%04x", Version);
    for (;;) {
        notify();
        osDelay(500);
    }
}

void StatusTask::notify() {
    uint8_t sw = sensor->read_value();
    int8_t temp = 1;
    auto err = ds18b20->read_value(temp);
    if (err) {
        sw |= 0x40;
    } else {
        sw &= ~0x40;
    }

    uint16_t p1 = robotArm1->get_position();
    uint16_t p2 = robotArm2->get_position();
    uint16_t s1 = fridge->get_speed();
    uint16_t s2 = tube->get_speed();
    uint16_t ps1 = fridge->get_count();
    uint16_t ps2 = tube->get_count();
    
    write(
        proto::StatusUpload | proto::NotifyFlag,
        Version,
        sw,
        temp,
        p1, p2,
        s1, s2,
        ps1, ps2
    );
}

}
